#include "ImageProgress.h"
#include "REG.h"
#include "serial.h"
#include "serialPort.h"
#include "wit_c_sdk.h"
#include <iostream>
#include <stdint.h>
#include <stdio.h>
#include <wiringPi.h>

// #define Vedio

// 播放顺序常量定义
constexpr bool kPlayForward = true; // true 为正序播放，false 为逆序播放

#define Quaternion_UPDATE 0x01

int frame_count = 0;

int fd, s_iCurBaud = 9600;
int fd2, s_iCurBaud2 = 9600;
volatile char s_cDataUpdate = 0;

const int c_uiBaud[] = {2400, 4800, 9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600};

static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum);
static void Delayms(uint16_t ucMs);

static void SensorDataUpdata(uint32_t uiReg, uint32_t uiRegNum)
{
    int i;
    for (i = 0; i < uiRegNum; i++)
    {
        switch (uiReg)
        {
        case q0:
        {
            s_cDataUpdate |= Quaternion_UPDATE;
        }
        }
        uiReg++;
    }
}

static void Delayms(uint16_t ucMs)
{
    usleep(ucMs * 1000);
}

void AutoScanSensor(unsigned char *dev)
{
    int i, iRetry;
    unsigned char cBuff[1];

    for (i = 1; i < 10; i++)
    {
        serial_close(fd);
        s_iCurBaud = c_uiBaud[i];
        fd = serial_open(dev, c_uiBaud[i]);

        iRetry = 2;
        do
        {
            s_cDataUpdate = 0;
            WitReadReg(AX, 3);
            Delayms(200);
            while (serial_read_data(fd, cBuff, 1))
            {
                WitSerialDataIn(cBuff[0]);
            }
            if (s_cDataUpdate != 0)
            {
                printf("%d baud find sensor\r\n\r\n", c_uiBaud[i]);
                return;
            }
            iRetry--;
        } while (iRetry);
    }
    printf("can not find sensor\r\n");
    printf("please check your connection\r\n");
}

ImageProgress::ImageProgress(Observer &_observer) : observer(_observer), detector(_observer)
{
    fitting_lck = std::unique_lock<std::mutex>(fitting_mutex);

    int color;
    cv::FileStorage fs("./../Configure/Settings.xml", cv::FileStorage::READ);
    fs["enemy_color"] >> color;
    std::cout << "enemy_color:color";
    fs.release();
    // cap.resetParam("./../Configure/Settings.xml");
    //  // TODO 在这要进行gpio的读取
    //  int gpio_success=wiringPiSetup();

    // pinMode(7, INPUT);
    // pullUpDnControl(7,PUD_UP);
    // int color_pin=digitalRead(7);
    // if(color_pin==1) //读到了高电平
    // {
    //     color=1;
    // }else if(color_pin==0) //读取到了低电平
    // {
    //     color=2;
    // }
    // std::cout<<"enemy_color:"+to_string(digitalRead(7))<<endl;

    _enemy_color = (COLOR)color;
}

void ImageProgress::ImageProducer()
{
#ifdef Vedio
    // cv::VideoCapture cap = cv::VideoCapture("./../video_save.avi");
    std::string vedio_file_name = std::string("./../video_save_fast.avi");
    // std::string vedio_file_name = std::string("./../video_save_medium.avi");
    // std::string vedio_file_name = std::string("./../video_save_fast.avi");
    cv::VideoCapture cap = cv::VideoCapture(vedio_file_name.c_str());
    // // cv::VideoCapture cap = cv::VideoCapture(0);
    std::cout << " camera success!!" << '\n';
#endif

    if (this->_mode == Mode::RUNE || this->_mode == Mode::NORMAL_RUNE)
    {
    }
    // cap.resetParam("./../Configure/Settings.xml");
    while (1)
    {
        Mat_time frame;
        Mat_time dsty;

        Mat_time adjusted;
#ifdef Vedio
        // 新增：内存加载与帧控制逻辑
        static int frame_index = 0;
        static std::vector<Mat> video_frames;
        if (video_frames.empty())
        {
            cv::Mat temp;
            while (cap.read(temp))
            {
                video_frames.push_back(temp.clone());
            }
            cap.release();
            std::cout << "Loaded " << video_frames.size() << " frames into memory.\n";
        }

        if (video_frames.empty())
            break;

        if (kPlayForward)
        {
            if (frame_index >= static_cast<int>(video_frames.size()))
                frame_index = 0;
        }
        else
        {
            if (frame_index < 0)
                frame_index = static_cast<int>(video_frames.size()) - 1;
        }

        frame = Mat_time(video_frames[frame_index]);

        frame_index += (kPlayForward ? 1 : -1);

        // 原有cap.read(frame)逻辑已被注释或删除
        /*
        if (!cap.read(frame))
        {
            std::cout << "Video reached end, reopening..." << std::endl;
            cap.release();                     // 释放旧的
            cap.open(vedio_file_name.c_str()); // 重新打开
            if (!cap.isOpened())
            {
                std::cerr << "Failed to reopen video file!" << std::endl;
                break; // 或者 continue 视情况
            }
            continue;
        }
        */
#else
        try
        {
            frame.create(ACQ_FRAME_HEIGHT, ACQ_FRAME_WIDTH, CV_8UC3);
            cap >> frame;
            // frame.convertTo(adjusted, -1, 0.5, 0); // 调整亮度
            // cv::flip(frame,dsty,-1);
            // frame=dsty;
            // frame=adjusted;

            // std::cout << "success!!" << '\n';
        }
        catch (const cv::Exception e)
        {
            std::cerr << e.what() << '\n';
            continue;
        }
#endif

        // std::cout << "frame_buffer.size:" << frame_buffer.size() << endl;
        if (frame.empty())
        {
            continue;
        }
        frame_mutex.lock();
        frame_buffer.push(frame); //将帧压栈到buffer中
        while (frame_buffer.size() > FRAME_BUFFER_NUM)
        {
            frame_buffer.pop();
        }
        frame_mutex.unlock();

#ifdef Vedio
        this_thread::sleep_for(chrono::milliseconds(10));
#else
        this_thread::sleep_for(chrono::milliseconds(1));
#endif
    }
}

void ImageProgress::ImageConsumer2()
{
    std::chrono::steady_clock::time_point last_time = std::chrono::steady_clock::now();
    RuneDetector rune_detector;

    while (1)
    {
        Mat_time frame;
        if (debug_param.debug_no_gyro) // 无IMU模式
        {
            while (frame_buffer.empty())
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
            frame_mutex.lock();
            frame = frame_buffer.front(); // 一帧一帧数地从队列出弹出来
            frame.gyro_pose.timestamp = (double)cv::getTickCount() / cv::getTickFrequency() * 1000.0;
            frame_buffer.pop();
            frame_mutex.unlock();
        }
        else
        {
            while (!InterGyroPose(frame)) // TODO 这里给帧附上陀螺仪的数据  等待can数据的到来
                std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
        if (_mode == Mode::NORMAL)
        {
            NormalConsumer(detector, frame, last_time);
        } //主要针对的就是这里的Normal模式 //TODO 这对frame进行了第一次入口
        else
        {
            RuneConsumer(rune_detector, frame, last_time);
        }

        // if (_mode == Mode::NORMAL)
        // {
        //     armor_state = detector.state;
        //     send_target_id = detector.target.id;
        // }
        // else
        //     armor_state = rune_detector.state;
    }
}

void ImageProgress::NormalConsumer(ArmorDetector &detector, Mat_time frame,
                                   std::chrono::steady_clock::time_point &last_time)
{
    detector.setEnemyColor(_enemy_color);
    detector.gpio_color = _enemy_color;
    detector.predictor.setBulletSpeed(_bullet_speed);

    cv::Point2f pitch_yaw; // cv::Point2f 是 OpenCV 中的一个类
    // 检测装甲板
    detector.run(frame, pitch_yaw);

    send_mutex.lock();
    send_pitch_yaw[0] = std::round(pitch_yaw.x * 10) / 10.0f;
    send_pitch_yaw[1] = std::round(pitch_yaw.y * 10) / 10.0f;
    cout << "send_Pitch: " << send_pitch_yaw[0] << "send_Yaw: " << send_pitch_yaw[1] << endl;
    send_time = frame.gyro_pose.timestamp;

    armor_state = detector.state;
    cout << "armor_state" << armor_state << endl;
    send_target_id = detector.target.id;

    send_mutex.unlock();

    if (debug_param.debug_speed)
    {
        double frame_time =
            std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::steady_clock::now() - last_time)
                .count() /
            1000.0; // 单位ms

        // 更新时间窗口
        detector.frame_times.push_back(frame_time);
        if (detector.frame_times.size() > ArmorDetector::frame_window_size)
        {
            detector.frame_times.pop_front();
        }

        // 计算平均帧时间
        double avg_frame_time = 0.0;
        for (auto t : detector.frame_times)
        {
            avg_frame_time += t;
        }
        avg_frame_time /= detector.frame_times.size();

        // 平均FPS
        detector.FPS_All = (int)(1000.0 / avg_frame_time);
        last_time = std::chrono::steady_clock::now();

        cout << "FPS:" << detector.FPS_All << endl;

        // std::cout
        // << "FPS: "
        //           << 1000000.0 /
        //                  std::chrono::duration_cast<chrono::microseconds>(std::chrono::steady_clock::now() -
        //                  last_time)
        //                      .count()
        //           << endl;
        // detector.FPS_All =
        //     (int)1000000.0 /
        //     std::chrono::duration_cast<chrono::microseconds>(std::chrono::steady_clock::now() - last_time).count();
        // std::cout << endl;
        // last_time = std::chrono::steady_clock::now();
    }

    if (debug_param.debug)
    {
        show_armor_result(frame, detector, debug_param, observer);

        if (waitKey(1) == 'q')
            exit(0);
    }
}

void ImageProgress::RuneConsumer(RuneDetector &detector, Mat_time frame,
                                 std::chrono::steady_clock::time_point &last_time)
{
    detector.setEnemyColor(_enemy_color);
    detector.run(frame);

    rune_mutex.lock();
    RuneSegament = 1;
    armor_state = detector.state;
    new_rune_armor = RuneArmor(detector.target);
    rune_gyro_pose = GyroPose(frame.gyro_pose);
    rune_mutex.unlock();

    if (debug_param.debug_speed)
    {
        std::cout
            << "Tick: "
            << std::chrono::duration_cast<chrono::microseconds>(std::chrono::steady_clock::now() - last_time).count()
            << endl;
        std::cout << endl;
        last_time = std::chrono::steady_clock::now();
    }

    if (debug_param.debug)
    {
        show_armor_result(frame, detector, rune_next_pos, debug_param);
        if (waitKey(1) == 'q')
            exit(0);
    }
}

void ImageProgress::Fitting()
{
    Solver solver;
    CoordPredictor predictor;
    FitTool fittool;
    while (true)
    {
        if (_mode == Mode::NORMAL)
        {
            fittool.clearData();
            fitting_condition.wait(fitting_lck);
            send_mutex.lock();
            send_pitch_yaw[0] = 0;
            send_pitch_yaw[1] = 0;
            send_mutex.unlock();
        }

        switch (_mode)
        {
        case RUNE:
            cout << "RUNE" << endl;
            break;

        case NORMAL_RUNE:
            cout << "NORMAL RUNE" << endl;
            break;

        default:
            cout << "WRONG" << endl;
            break;
        }
        while (RuneSegament <= 0)
        {
            this_thread::sleep_for(std::chrono::milliseconds(1));
        }
        rune_mutex.lock();
        RuneSegament--;
        RuneArmor fit_new = new_rune_armor;
        GyroPose fit_gyro_pose = rune_gyro_pose;
        fit_new.gyro_pose = rune_gyro_pose;
        new_rune_armor = RuneArmor();
        rune_gyro_pose = GyroPose();
        rune_mutex.unlock();

        rune_next_pos.clear();
        if (fittool.run(fit_new, rune_next_pos, armor_state, _mode))
        {
            solver.solve(rune_next_pos, ArmorType::RUNE_ARMOR);
            // 抬枪补偿
            Eigen::Quaternionf q(fit_gyro_pose.q_0, fit_gyro_pose.q_1, fit_gyro_pose.q_2, fit_gyro_pose.q_3);
            cv::Point3f world_coord = camera2world(q, solver.point_3D);

            predictor.setBulletSpeed(30);
            predictor.setBS_coeff(world_coord);
            cv::Point2f pitch_time = predictor.compensate(world_coord, 0.005);
            // cout << "SolvePnP: " << solver.point_3D << endl;
            // cout << "Pitch Time: " << pitch_time << endl;
            cout << "World Coord: " << world_coord << endl;
            send_mutex.lock();
            send_pitch_yaw[0] =
                -solver.pitch - pitch_time.x +
                atan(world_coord.z / sqrt(world_coord.x * world_coord.x + world_coord.y * world_coord.y)) * 180.0 /
                    CV_PI +
                1.0;
            send_pitch_yaw[1] = solver.yaw - 0.8;
            cout << "send_Pitch: " << send_pitch_yaw[0] << "send_Yaw: " << send_pitch_yaw[1] << endl;
            send_time = fit_new.gyro_pose.timestamp;
            send_mutex.unlock();

            fittool.printResult();
        }
    }
}

bool ImageProgress::InterGyroPose(Mat_time &frame)
{
    while (gyro_buffer.size() < GYRO_BUFFER_NUM)
        this_thread::sleep_for(chrono::milliseconds(1));
    cout << "gyro_buffer.size is " << gyro_buffer.size() << "finished!!!" << endl;
    gyro_mutex.lock();
    GyroPose temp_gyro_buffer[GYRO_BUFFER_NUM];
    for (int i = 0; i < GYRO_BUFFER_NUM; i++)
        temp_gyro_buffer[i] = gyro_buffer[i]; //在对陀螺仪数据进行复制 先收集15个陀螺仪数据
    gyro_mutex.unlock();

    while (frame_buffer.empty())
    {
        this_thread::sleep_for(chrono::milliseconds(1));
    }
    frame_mutex.lock();
    cout << "frame buffer size  in pop is" << frame_buffer.size(); //拿出一个帧 来匹配陀螺仪数据
    while (!frame_buffer.empty())
    {
        Mat_time temp = frame_buffer.front(); //拿出一个帧 来匹配陀螺仪数据
        frame_buffer.pop();

        uint64_t delta[GYRO_BUFFER_NUM] = {0};
        for (int i = 0; i < GYRO_BUFFER_NUM; i++) //来计算陀螺仪数据应该选哪个给帧数
        {
            delta[i] =
                chrono::duration_cast<chrono::microseconds>(temp_gyro_buffer[i].local_timestamp - temp.produced_time)
                    .count();
            // cout<<"temp.produced_time is"<<delta[i]<<endl;
        }
        // if (delta[0] > 0) // 图像远滞后于陀螺仪数据    //第一个因该小于0 就是
        // 陀螺仪数据--陀螺仪数据--陀螺仪数据--《帧》--陀螺仪数据--陀螺仪数据--陀螺仪数据
        // {
        //     return true;
        // }
        // else
        if (delta[GYRO_BUFFER_NUM - 1] >= 0)
        {
            int min_index = 0;
            int64 min_value = abs(delta[0]);
            for (int i = 1; i < GYRO_BUFFER_NUM; i++)
            {
                if (abs(delta[i]) < min_value)
                {
                    min_value = abs(delta[i]);
                    min_index = i;
                }
            }
            frame = temp;
            frame.gyro_pose = temp_gyro_buffer[min_index];

            frame_mutex.unlock();
            return true;
        }
        else if (delta[GYRO_BUFFER_NUM - 1] < 0)
        {
            if (delta[GYRO_BUFFER_NUM - 1] > -1000)
            {
                frame = temp;
                frame.gyro_pose = temp_gyro_buffer[GYRO_BUFFER_NUM - 1];

                frame_mutex.unlock();
                return true;
            }

            frame_mutex.unlock();
            return false;
        }
    }
    frame_mutex.unlock();
    return false;
}

void ImageProgress::ReadIMU2()
{
    // CanSerial::CanError state;
    std::chrono::steady_clock::time_point last_read_time = std::chrono::steady_clock::now();
    //使用wit陀螺仪的SDK
    WitInit(WIT_PROTOCOL_NORMAL, 0x50);
    WitRegisterCallBack(SensorDataUpdata);
    printf("\r\n********************** wit-gyro finding  ************************\r\n");
    const char *str = "/dev/ttyS6"; // 字符串字面量
    unsigned char *uchar_str = reinterpret_cast<unsigned char *>(const_cast<char *>(str));
    AutoScanSensor(uchar_str);
    float imu_data[4] = {0};
    unsigned char cBuff[1];
    uint32_t timestamp = 0, temp_timestamp = 0;
    // auto rune_signal_first_get_timestamp = chrono::steady_clock::now(); // 接收到打符信号后x秒再退出

    while (true)
    {
        uint id = 0;
        u_char dlc = 0;
        u_char buf[8] = {0};
        uint16_t competition_remain_time;

        if (debug_param.debug_serial_frequency_read)
        {
            std::cout << "ReadFreq: "
                      << std::chrono::duration_cast<chrono::microseconds>(std::chrono::steady_clock::now() -
                                                                          last_read_time)
                             .count()
                      << endl;
            last_read_time = std::chrono::steady_clock::now();
        }
        while (serial_read_data(fd, cBuff, 1))
        {
            WitSerialDataIn(cBuff[0]);
        }

        if (s_cDataUpdate)
        {
            for (int i = 0; i < 4; i++)
            {
                imu_data[i] = sReg[q0 + i] / 32768.0f;
            }
            if (s_cDataUpdate & Quaternion_UPDATE)
            {
                gyro_mutex.lock();
                newest_pose = GyroPose(imu_data, timestamp);
                gyro_buffer.push_back(newest_pose);
                while (gyro_buffer.size() > GYRO_BUFFER_NUM)
                    gyro_buffer.erase(gyro_buffer.begin());
                gyro_mutex.unlock();

                if (debug_param.debug_gyro_data)
                {
                    std::cout << "四元数：" << std::endl;
                    std::cout << "q0: " << newest_pose.q_0 << "     ";
                    std::cout << "q1: " << newest_pose.q_1 << endl;
                    std::cout << "q2: " << newest_pose.q_2 << "     ";
                    std::cout << "q3: " << newest_pose.q_3 << endl;
                    std::cout << "Time: " << newest_pose.timestamp << endl;
                    std::cout << endl;
                }

                s_cDataUpdate &= ~Quaternion_UPDATE;
            }
            if (temp_timestamp < 5000000)
            {
                if ((double)temp_timestamp - (double)timestamp < -10 && timestamp != 0)
                {
                    // cout << "TEMP: "<<  temp_timestamp << endl;
                    // cout << "time: " << timestamp << endl;
                    // exit(0);
                }
                // timestamp = temp_timestamp;
                timestamp = timestamp + 1;
            }
            else
            {
                timestamp = timestamp + 1;
            }
        }
        this_thread::sleep_for(chrono::microseconds(50));
    }
}

void ImageProgress::SendData2()
{

    typedef struct
    {
        struct frame_header
        {
            uint8_t sof;  // 数据帧起始字节，固定值为 0xA5
            uint8_t len;  // 数据段长度
            uint8_t id;   // 数据段id
            uint8_t type; // 数据段类型
            uint8_t crc;  // 数据帧头的 CRC8 校验
        } __attribute__((packed)) frame_header;

        uint8_t data[10];
        uint16_t crc;
    } __attribute__((packed)) BoardCommunicateData_s;
    BoardCommunicateData_s BOARD_TX_DATA;
    uint8_t fact_data[20];
    u_char PY_data[4];
    const char *dev = "/dev/ttyS1";
    chrono::steady_clock::time_point serial_last_send = chrono::steady_clock::now();
    serialPort myserial;
    int i, nread, nwrite;
    myserial.OpenPort(dev);
    myserial.setup(460800, 0, 8, 1, 'N');
    std::string message =
        "p:" + std::to_string(send_pitch_yaw[0]) + "y:" + std::to_string(send_pitch_yaw[1]) + "fire" + "\n";
    // std::string message = std::to_string(BO) +std::to_string(send_pitch_yaw[1]) +
    //             "fire"+"\n";
    // u_char PY_data[4];

    while (true)
    {
        BOARD_TX_DATA.frame_header.sof = 0xA5; // 数据帧起始字节，固定值为 0xA5
        BOARD_TX_DATA.frame_header.len = 10;   // 数据帧中数据段的长度
        BOARD_TX_DATA.frame_header.id = 0x03;
        BOARD_TX_DATA.frame_header.type = 0x01; //数据段类型先空着，默认0x01
        // transformData(pitch,yaw);
        //  send_pitch_yaw[0]=0;
        //  send_pitch_yaw[1]=-30;
        //  for(int i=1;i<=3;(i++)%3){
        //      send_pitch_yaw[1]=-i*30;

        int16_t pitch_val = (int16_t)(send_pitch_yaw[0] / 32.0 * 32768.0 / 180.0);
        int16_t yaw_val = (int16_t)(send_pitch_yaw[1] / 32.0 * 32768.0 / 180.0);
        for (int i = 0; i < 4; i++)
            PY_data[i] = 0;

        PY_data[0] = (yaw_val >> 8) & 0xFF;
        PY_data[1] = yaw_val & 0xFF;
        PY_data[2] = (pitch_val >> 8) & 0xFF;
        PY_data[3] = pitch_val & 0xFF;
        // if(PY_data) cout<<"PY_data is right :"<<endl;
        // cout<<"PY_data[0]="<<(uint8_t)PY_data[0]<<",PY_data[1]="<<(uint8_t)PY_data[1]<<",PY_data[2]="<<(uint8_t)PY_data[2]<<",PY_data[3]="<<(uint8_t)PY_data[3]<<endl;
        fact_data[0] = PY_data[0];
        fact_data[1] = PY_data[1];
        fact_data[2] = PY_data[2];
        fact_data[3] = PY_data[3];
        // armor_state=detector.state;
        // std::cout<<"state"<<armor_state<<std::endl;
        fact_data[4] = armor_state;

        // 添加帧头 CRC8 校验位
        append_CRC8_check_sum((uint8_t *)(&BOARD_TX_DATA.frame_header), sizeof(BOARD_TX_DATA.frame_header));

        memcpy(BOARD_TX_DATA.data, fact_data, 10); // 数据段

        // 帧尾CRC16，整包校验
        append_CRC16_check_sum((uint8_t *)(&BOARD_TX_DATA), sizeof(BOARD_TX_DATA));
        // message = "p:" + std::to_string(send_pitch_yaw[0]) +
        //     "y:" + std::to_string(send_pitch_yaw[1]) +
        //     "fire"+"\n";
        // send_mutex.lock();  //为什么要删掉呢就是因为发送线程的速度太快了 频繁的上锁 阻塞了数据的正常传输
        // std::cout<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<std::endl;
        // std::cout<<"------------send_pitch_yaw[0]:"<<send_pitch_yaw[0]<<","<<"send_pitch_yaw[1]:"<<send_pitch_yaw[1]<<"----------------"<<std::endl;
        // std::cout<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<std::endl;
        // nwrite = myserial.writeBuffer((uint8_t*)message.c_str(), message.size());
        // nwrite = myserial.writeBuffer((uint8_t*)BOARD_TX_DATA, 20);
        nwrite = myserial.writeBuffer(reinterpret_cast<uint8_t *>(&BOARD_TX_DATA), 17);
        // send_mutex.unlock();
        //  nwrite = myserial.writeBuffer(
        //  reinterpret_cast<uint8_t*>(&BOARD_TX_DATA),  // 强制转换为字节流指针
        //  sizeof(BOARD_TX_DATA)                 // 发送结构体实际大小
        //  );
        this_thread::sleep_for(chrono::microseconds(400));
        // nwrite = myserial.writeBuffer((uint8_t*)"p:10,y:20,fire", strlen("p:10,y:20,fire"));

        // can_serial.send_data(send_pitch_yaw, this->send_time);
        // send_mutex.unlock();

        // if (state != CanSerial::TIME_ERROR && debug_param.debug_serial_frequency_send)
        // {
        //     std::cout << "Send Freq: " << chrono::duration_cast<chrono::microseconds>(chrono::steady_clock::now() -
        //     serial_last_send).count() << endl; serial_last_send = chrono::steady_clock::now();
        // }       // //can_serial.send_state(armor_state, send_target_id);
        //}
    }
}

void ImageProgress::VideoSave()
{
    string filename = "./video_save.avi";
    cv::VideoWriter video_writer(filename, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'), 30.0,
                                 Size(ACQ_FRAME_WIDTH, ACQ_FRAME_HEIGHT));
    while (true)
    {
        if (!recording_enabled)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            continue;
        }

        while (frame_buffer.empty())
            std::this_thread::sleep_for(std::chrono::milliseconds(30));

        frame_mutex.lock();
        Mat frame = frame_buffer.front();
        frame_mutex.unlock();
        video_writer << frame;
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

ArmorDetector &ImageProgress::getArmorDetector()
{
    return this->detector;
}